package team3132.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.command.Subsystem;
import team3132.commands.DriveSubsystemIdleCommand;

/**
 * Provides access to the RobotDrive which provides access to the drive motors.
 */
public class DriveSubsystem extends Subsystem {

  private final SpeedController leftMotor;
  private final SpeedController rightMotor;
  private final Encoder leftEncoder;
  private final Encoder rightEncoder;
  private final RobotDrive drive;

  public DriveSubsystem(SpeedController leftMotor, SpeedController rightMotor, Encoder leftEncoder, Encoder rightEncoder) {
    this.leftMotor = leftMotor;
    this.rightMotor = rightMotor;
    this.leftEncoder = leftEncoder;
    this.rightEncoder = rightEncoder;

    drive = new RobotDrive(leftMotor, rightMotor);
  }

  public void initDefaultCommand() {
    // Set the default command for a subsystem here.
    setDefaultCommand(new DriveSubsystemIdleCommand(this));
  }

  public void drive(double move, double rotate) {
    drive.drive(move, rotate);
  }

  public void arcadeDrive(double move, double rotate) {
    drive.arcadeDrive(move, rotate);
  }

  public void tankDrive(double move, double rotate) {
    drive.tankDrive(move, rotate);
  }

  public void resetLeftEncoderDistance() {
    leftEncoder.reset();
  }

  public void resetRightEncoderDistance() {
    rightEncoder.reset();
  }

  public double getLeftEncoderDistance() {
    return leftEncoder.getDistance();
  }

  public double getRightEncoderDistance() {
    return rightEncoder.getDistance();
  }

  public double getLeftEncoderCount() {
    return leftEncoder.get();
  }

  public double getRightEncoderCount() {
    return rightEncoder.get();
  }

  public void set(double speed) {
    drive.drive(speed, 0);
  }

  /**
   * Get the speed in meters per second.
   */
  public double getSpeed() {
    // One encoder will be going forwards and the other backwards, so take
    // the absolute value.
    return (Math.abs(leftEncoder.getRate()) + Math.abs(rightEncoder.getRate())) / 2.0;
  }
}
